#include "modbus_handler.h"

int modbus_slave_handler_HMIhandler_ReadHoldReg(modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    tableErr_t ret = 0;
    int index = 0;
    int number = modbus_info->number;
    uint32_t addr = modbus_info->addr;
    uint16_t ivalue = 0;
    for (index = 0; index < number; number--, addr++)
    {
        ret = HmiTable_Get(addr, &ivalue);
        if(ret != TABLE_ERR_NONE)
        {
            PRINT_ERROR("HmiTable_Get fail addr[%d]", addr);
            return -1;
        }
        mb_map->tab_registers[addr] = ivalue;
        // printf("mb_map->tab_registers[%d] = %d\n", addr, mb_map->tab_registers[addr]);
    }
    return number;
}

int modbus_slave_handler_HMIhandler_WriteSingleReg(modbus_mapping_t *mb_map, struct modbus_info_s * modbus_info)
{
    tableErr_t ret = 0;
    uint32_t addr = modbus_info->addr;
    uint16_t data = modbus_info->data;
    ret = HmiTable_Set(addr, data);
    if(ret != TABLE_ERR_NONE)
    {
        PRINT_ERROR("HmiTable_Set fail addr[%d] data[%d]", addr, data);
        return -1;
    }
    return 1;
}

int modbus_slave_handler_HMIhandler_WriteMultiReg(const uint8_t *req, modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    int ret = 0;
    int addr = req[2] << 8 | req[3];
    int num = req[4] << 8 | req[5];
    int data = req[7] << 8 | req[8];
    int i = 7;

    while (num)
    {
        modbus_info->addr = addr;
        modbus_info->data = data;
        ret = modbus_slave_handler_HMIhandler_WriteSingleReg(mb_map, modbus_info);
        if(ret == -1)
        {
            PRINT_ERROR("modbus_slave_handler_HMIhandler_WriteSingleReg fail addr[%d] data[%d]", addr, data);
            return -1;
        }
        addr++;
        i += 2;
        data = req[i] << 8 | req[i + 1];
        num -= 1;
    }
    return num;
}

int modbus_slave_handler_HMIhandler(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    int ret = 0;
    switch (modbus_info->functions)
    {
        case MODBUS_FC_READ_HOLDING_REGISTERS:
        case MODBUS_FC_READ_INPUT_REGISTERS:
            ret = modbus_slave_handler_HMIhandler_ReadHoldReg(mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_HMIhandler_ReadHoldReg fail");
            }
            break;
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            ret = modbus_slave_handler_HMIhandler_WriteSingleReg(mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_HMIhandler_WriteSingleReg fail");
            }
            break;
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            ret = modbus_slave_handler_HMIhandler_WriteMultiReg(req, mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_HMIhandler_WriteMultiReg fail");
            }
            break;
        default:
            printf("[%s][%d] modbus fc error!\n", __func__, __LINE__);
            break;
    }
    return ret;
}


int modbus_slave_handler_EMShandler_ReadInputReg(modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    tableErr_t ret = 0;
    int index = 0;
    int number = modbus_info->number;
    uint32_t addr = modbus_info->addr;
    uint16_t ivalue = 0;
    for (index = 0; index < number; number--, addr++)
    {
        ret = EmsTable_Get(addr, &ivalue);
        if(ret != TABLE_ERR_NONE)
        {
            PRINT_ERROR("EmsTable_Get fail addr[%d]", addr);
            return -1;
        }
        mb_map->tab_input_registers[addr] = ivalue; 
    }
    return number;
}

int modbus_slave_handler_EMShandler_ReadHoldReg(modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    tableErr_t ret = 0;
    int index = 0;
    int number = modbus_info->number;
    uint32_t addr = modbus_info->addr;
    uint16_t ivalue = 0;
    for (index = 0; index < number; number--, addr++)
    {
        ret = EmsTable_Get(addr, &ivalue);
        if(ret != TABLE_ERR_NONE)
        {
            PRINT_ERROR("EmsTable_Get fail addr[%d]", addr);
            return -1;
        }
        mb_map->tab_registers[addr] = ivalue; 
    }
    return number;
}

int modbus_slave_handler_EMShandler_WriteSingleReg(modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    tableErr_t ret = 0;
    uint32_t addr = modbus_info->addr;
    uint16_t data = modbus_info->data;
    ret = EmsTable_Set(addr, data);
    if(ret != 0)
    {
        PRINT_ERROR("EmsTable_Set fail addr[%d] data[%d]", addr, data);
        return -1;
    }
    return 1;
}

int modbus_slave_handler_EMShandler_WriteMultiReg(const uint8_t *req, modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    int ret = 0;
    int addr = req[8] << 8 | req[9];
    int num = req[10] << 8 | req[11];
    int data = req[13] << 8 | req[14];
    int i = 13;

    while (num--)
    {
        modbus_info->addr = addr;
        modbus_info->data = data;
        ret = modbus_slave_handler_EMShandler_WriteSingleReg(mb_map, modbus_info);
        if(ret == -1)
        {
            PRINT_ERROR("modbus_slave_handler_EMShandler_WriteSingleReg fail addr[%d] data[%d]", addr, data);
            return -1;
        }
        addr++;
        i += 2;
        data = req[i] << 8 | req[i + 1];
    }
    return num;
}

int modbus_slave_handler_EMShandler(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    int ret = 0;
    switch (modbus_info->functions)
    {
        case MODBUS_FC_READ_HOLDING_REGISTERS:
            ret = modbus_slave_handler_EMShandler_ReadHoldReg(mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_EMShandler_ReadHoldReg fail");
            }
            break;
        case MODBUS_FC_READ_INPUT_REGISTERS:
            ret = modbus_slave_handler_EMShandler_ReadInputReg(mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_EMShandler_ReadInputReg fail");
            }
            break;
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            ret = modbus_slave_handler_EMShandler_WriteSingleReg(mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_EMShandler_WriteSingleReg fail");
            }
            break;
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            ret = modbus_slave_handler_EMShandler_WriteMultiReg(req, mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_EMShandler_WriteMultiReg fail");
            }
            break;
        default:
            printf("[%s][%d] modbus fc error!\n", __func__, __LINE__);
            break;
    }
    return ret;
}

int modbus_slave_handler_LChandler_ReadHoldReg(modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    tableErr_t ret = 0;
    int index = 0;
    int number = modbus_info->number;
    uint32_t addr = modbus_info->addr;
    uint16_t ivalue = 0;
    for (index = 0; index < number; number--, addr++)
    {
        ret = LcTable_Get(addr, &ivalue);
        if(ret != TABLE_ERR_NONE)
        {
            PRINT_ERROR("LcTable_Get fail addr[%d]", addr);
            return -1;
        }
        mb_map->tab_registers[addr] = ivalue;
    }
    return number;
}

int modbus_slave_handler_LChandler_WriteSingleReg(modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    tableErr_t ret = 0;
    uint32_t addr = modbus_info->addr;
    uint16_t data = modbus_info->data;
    ret = LcTable_Set(addr, data);
    if(ret != TABLE_ERR_NONE)
    {
        PRINT_ERROR("LcTable_Set fail addr[%d] data[%d]", addr, data);
        return -1;
    }
    return 1;
}

int modbus_slave_handler_LChandler_WriteMultiReg(const uint8_t *req, modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    int ret = 0;
    int addr = req[8] << 8 | req[9];
    int num = req[10] << 8 | req[11];
    int data = req[13] << 8 | req[14];
    int i = 13;

    while (num--)
    {
        modbus_info->addr = addr;
        modbus_info->data = data;
        ret = modbus_slave_handler_LChandler_WriteSingleReg(mb_map, modbus_info);
        if(ret == -1)
        {
            PRINT_ERROR("modbus_slave_handler_LChandler_WriteSingleReg fail addr[%d] data[%d]", addr, data);
            return -1;
        }
        addr++;
        i += 2;
        data = req[i] << 8 | req[i + 1];
    }
    return ret;
}

int modbus_slave_handler_LChandler(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    int ret = 0;
    switch (modbus_info->functions)
    {
        case MODBUS_FC_READ_HOLDING_REGISTERS:
        case MODBUS_FC_READ_INPUT_REGISTERS:
            ret = modbus_slave_handler_LChandler_ReadHoldReg(mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_LChandler_ReadHoldReg fail");
            }
            break;
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            ret = modbus_slave_handler_LChandler_WriteSingleReg(mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_LChandler_WriteSingleReg fail");
            }
            break;
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            ret = modbus_slave_handler_LChandler_WriteMultiReg(req, mb_map, modbus_info);
            if(ret == -1)
            {
                PRINT_ERROR("modbus_slave_handler_LChandler_WriteMultiReg fail");
            }
            break;
        default:
            printf("[%s][%d] modbus fc error!\n", __func__, __LINE__);
            break;
    }
    return ret;
}


int modbus_recv_message_handler(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_map, struct modbus_info_s *modbus_info)
{
    // printf("modbus_info->name is %s\n",modbus_info->name);
    // printf("modbus_info->addr is %d\n",modbus_info->addr);
    // printf("modbus_info->number is %d\n",modbus_info->number);
    // printf("modbus_info->data is %d\n",modbus_info->data);
    // printf("modbus_info->functions is %d\n",modbus_info->functions);

    int ret = 0;
    if (NULL == ctx || NULL == req || NULL == mb_map || NULL == modbus_info)
    {
        printf("[%s][%d] modbus_recv_message_handle para error\n", __func__, __LINE__);
        return -1;
    }

    if ((strcmp(modbus_info->name, "502") != 0 && strcmp(modbus_info->name, "505") != 0))
    {
        CommMonitor_AddHeartbeat(COMM_DEVICE_HMI);
        ret = modbus_slave_handler_HMIhandler(ctx, req, req_length, mb_map, modbus_info);
        if(ret == -1)
        {
            PRINT_ERROR("modbus_slave_handler_HMIhandler");
            return -1;
        }
    }else if (strcmp(modbus_info->name, "502") == 0)
    {
        CommMonitor_AddHeartbeat(COMM_DEVICE_EMS);
        ret = modbus_slave_handler_EMShandler(ctx, req, req_length, mb_map, modbus_info);
        if(ret == -1)
        {
            PRINT_ERROR("modbus_slave_handler_EMShandler");
            return -1;
        }
    }else if (strcmp(modbus_info->name, "505") == 0)
    {
        CommMonitor_AddHeartbeat(COMM_DEVICE_LC);
        ret = modbus_slave_handler_LChandler(ctx, req, req_length, mb_map, modbus_info);
        if(ret == -1)
        {
            PRINT_ERROR("modbus_slave_handler_LChandler");
            return -1;
        }
    }else
    {
        PRINT_ERROR("Error port");
        return -1;
    }    
    
    return ret;
}

int modbus_send_message_handler(modbus_t *ctx, Modbus_info_t *modbus_info, uint16_t *pBuf)
{
    // printf("modbus_info->sid is [%d] \n",modbus_info->sid);
    // printf("modbus_info->fc is [%d] \n",modbus_info->fc);
    // printf("modbus_info->addr is [%d] \n",modbus_info->addr);
    // printf("modbus_info->num is [%d] \n",modbus_info->num);
    // printf("modbus_info->val is [%d] \n",modbus_info->val);

    if (NULL == ctx || NULL == modbus_info)
    {
        printf("[%s][%d] modbus send message fail!\n", __func__, __LINE__);
        return 0;
    }

    int ret = 0;
    ret = modbus_set_slave(ctx, modbus_info->sid);
    if (ret < 0)
    {
        printf("[%s][%d] modbus set slave fail! ret is %d\n", __func__, __LINE__, ret);
        return 0;
    }
    
    switch (modbus_info->fc)
    {
        case MODBUS_FC_READ_HOLDING_REGISTERS:
            ret = modbus_read_registers(ctx, modbus_info->addr, modbus_info->num, pBuf);
            if(ret < 0)
            {
                printf("[%s][%d] modbus read fail! ret is %d\n", __func__, __LINE__, ret);
                return 0;    
            }
            break;
        
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            ret = modbus_write_register(ctx, modbus_info->addr, modbus_info->val);
            if(ret < 0)
            {
                printf("[%s][%d] modbus write fail! ret is %d\n", __func__, __LINE__, ret);
                return 0;
            }
            break;

        default:
            printf("[%s][%d] modbus fc error!\n", __func__, __LINE__);
            return 0;
    }

    return ret;
}